#!/usr/bin/python3

import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped
import time
import numpy as np
from std_msgs.msg import Float64MultiArray, Header
from sensor_msgs.msg import LaserScan, JointState
from gazebo_msgs.msg import ModelStates
import copy
from tf2_geometry_msgs import tf2_geometry_msgs
import sys
from pogo_group.srv import CalibratePogoGroup, CalibratePogoGroupResponse
import yaml
from pogo_device import pogo_device

class pogo_group_service():
    def __init__(self,nodename, saved_calibrate_file, ns_prefix):
        rospy.init_node(nodename)
        #rospy.Subscriber("/gazebo/model_states", ModelStates, self.cb_modelStates)
        #debug_prefix = "/pogo_group/"
        debug_prefix = ""

        self.pogo_group_num = rospy.get_param(debug_prefix + "pogo_group_config/single_pogo_num")

        joint_state_controller = {"type":"joint_state_controller/JointStateController",
            "publish_rate": 50.0}
        rospy.set_param(debug_prefix + "joint_state_controller", joint_state_controller)

        ptp_controller = {"type": "position_controllers/JointGroupPositionController"}
        ptp_controller['joints'] = []
        for i in range(self.pogo_group_num):
            ptp_controller['joints'].append("pogo" + str(i) + "_joint1")
            ptp_controller['joints'].append("pogo" + str(i) + "_joint2")
            ptp_controller['joints'].append("pogo" + str(i) + "_joint3")
        rospy.set_param(debug_prefix + "ptp_controller", ptp_controller)

        self.saved_calibrate_file = saved_calibrate_file
        rospy.Service(debug_prefix + "calibrate_pogo_group",CalibratePogoGroup, self.execute_calibration)

        # hardware_interface = rospy.get_param(debug_prefix + "pogo_group_config/hardware_interface")
        # self._device = pogo_device('192.168.20.128.1.1','192.168.20.128',hardware_interface)

        # self.js = JointState()
        # self.js.header = Header()
        # self.js.name = []
        # self.js.position = []
        # self.js.velocity = []
        # self.js.effort = []

        # self.pub = rospy.Publisher(debug_prefix + 'pogo_group_joint_states', JointState, queue_size=1)

    def execute_calibration(self,req):
        print(req.name)
        print(req.pose)

        print("file written to ",self.saved_calibrate_file)

        with open(self.saved_calibrate_file,'r+',encoding = 'utf-8') as pfile:
            output = yaml.load(pfile)

            output["pogo_group_config"]={}
            output['pogo_group_config']['single_pogo_num'] = len(req.name) 
            output['pogo_group_config']['type'] = req.type 

            born = []
            for i in range(len(req.pose)):
                single_record = [req.pose[i].position.x / 1000.0, req.pose[i].position.y / 1000.0, req.pose[i].position.z / 1000.0]
                single_rpy = tf.transformations.euler_from_quaternion(np.array([
                    req.pose[i].orientation.x,
                    req.pose[i].orientation.y,
                    req.pose[i].orientation.z,
                    req.pose[i].orientation.w
                    ]),axes='sxyz')
                single_record.extend(single_rpy[:])
                born.append(single_record)

            output['pogo_group_config']['born'] = born

            pfile.seek(0)

            yaml.dump(output,pfile)

        return CalibratePogoGroupResponse(isSucessful=True)

    def run(self):
        r = rospy.Rate(5)

        while (not rospy.is_shutdown()):
            # hardware_joints = self._device.readLegControlLength()

            # joint_state_positions = []
            # joint_state_name = [] 

            # for i in range(len(hardware_joints)):
            #     single_pogo_joint_names = []
            #     for j in range(len(hardware_joints[i])):
            #         single_pogo_joint_names.append("pogo"+str(i)+"_joint"+str(j+1))
            #         joint_state_positions.append(hardware_joints[i][j])
            #     joint_state_name.extend(single_pogo_joint_names)

            # self.js.name = joint_state_name
            # self.js.position = joint_state_positions
            # self.js.header.stamp = rospy.Time.now()
            # self.pub.publish(self.js)

            r.sleep()

if __name__ == '__main__':
    prefix = "pogo_group"
    calibration_file = "/home/zhangjiwen/catkin_dtsystem/src/aero_compiling_common/pogo_group/config/sample.yaml"

    if(len(sys.argv)>1):
        prefix = sys.argv[1]
        print("file written to prefix",prefix)

    if(len(sys.argv)>2):
        calibration_file = sys.argv[2]
        print("file written to ",calibration_file)

    server = pogo_group_service('pogo_group_service',calibration_file,prefix)
    # server.run()
    rospy.spin()
